package com.erau.droidsatframework.control;

import android.util.Log;

public class Algorithm {
	//Initialize Variables
	double velocity =0;
	int acceleration = 100;
	int motor = 0;
	int pVelocity;
	double target = 0;
	final double D = 2.5*(Math.PI/180.0);
	final double M =.5;
	final int MAX_VELOCITY=800;
	final double INCREMENT=.75;
	double lastVelocity=0;
	double lastAngle=0, lastRate=0;
	double totalAngle=0;
	double angleDeg, rateDeg;
	double error;
	double FUDGE=-.055;//
	final double cFUDGE = -.0551;
	boolean onTarget=false;
	boolean timeFlag = false;
	long baseTime =0;
	private static final String TAG = "Algorithm:";
	
	float xk_prev=0.0f;
	float Pk_prev=1.0f;
	float xk=0;
	final double noise=.006;
	public int[] algorithm(float angle, float rate) {
		
		angleDeg=angle*(180.0/Math.PI);
		totalAngle+=angleDeg;
		//Log.v("Total Angle:  ", Double.toString(totalAngle));//
		error=target-totalAngle;
		
		
		if (error<5 && error>-5) {
			onTarget=true;
			velocity=0;	
			if(!timeFlag){
				baseTime = System.currentTimeMillis();
				timeFlag = true;
			}
		}
		else if (error>0) {
			onTarget=false;
			velocity=-500;
		}
		else {
			onTarget=false;
			velocity=500;
		}
		
		if (onTarget && (System.currentTimeMillis()-baseTime) > 5000) {
			if(target == 0){
				target = 90;
				timeFlag = false;
			}
			else if(target == 90){
				target = 0;
				timeFlag = false;
			}

		}
		
		pVelocity=(int) Math.round(velocity);
		int[] values={pVelocity, acceleration, motor};
		return values;
		

	} 
	
	public double getTotalAngle (){
		return totalAngle;
	}
	
}
